#include "heartbeat_listener.h"
#include "heartbeat_model.h"
#include <chrono>
#include "global_logger.h"

/**
 * @brief 心跳监听器的构造函数
 * 
 * 该构造函数初始化了心跳监听器节点，设置了ROS参数，订阅了心跳消息，并创建了定时器以检查超时。
 * 
 * @param model 心跳模型的指针，用于更新模型数据
 */
HeartbeatListener::HeartbeatListener(HeartbeatModel* model) 
    : QObject(nullptr), 
      rclcpp::Node("heartbeat_listener"),
      model_(model) 
{
    // ROS参数配置
    this->declare_parameter<int>("timeout", 15); // 15秒作为离线超时时间
    this->get_parameter("timeout", timeout_);
    
    LOG_INFO("心跳监听器初始化，超时时间: {}秒", timeout_);
    
    // 使用更适合心跳传输的QoS配置
    auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
        .reliable()
        .durability_volatile();
    rclcpp::SubscriptionOptions options;
    subscription_ = this->create_subscription<rms_pkg::msg::Heartbeat>(
        "heartbeat", qos,
        [this](const rms_pkg::msg::Heartbeat::SharedPtr msg)  {
             heartbeat_callback(msg); 
            },
        options
    );
    
    LOG_DEBUG("创建心跳消息订阅器");
    
    // 超时检查定时器 - 设置为比心跳间隔稍长的时间
    timer_ = this->create_wall_timer(
        std::chrono::seconds(5), // 每5秒检查一次，比心跳间隔短
        [this]() { check_timeout(); });
    
    LOG_DEBUG("创建超时检查定时器");
}

/**
 * 心跳回调函数，用于处理接收到的心跳消息
 * @param msg 共享指针指向接收到的心跳消息
 * 
 * 本函数首先确定自上次发射以来是否已过去足够时间（至少200毫秒），以避免过于频繁地发射信号。
 * 如果时间条件满足，则记录当前时间，准备处理接收到的消息。
 * 消息包含机器人类型、机器人ID和时间戳，这些数据将被转换并用于发出信号。
 */
void HeartbeatListener::heartbeat_callback(const rms_pkg::msg::Heartbeat::SharedPtr msg) 
{
    using namespace std::chrono_literals; 
    QMutexLocker locker(&data_mutex_);
    static std::chrono::steady_clock::time_point lastEmit = std::chrono::steady_clock::now();
    auto now = std::chrono::steady_clock::now();
    if (now - lastEmit < 200ms) return;
   
    // 使用本地系统时间作为时间戳，而不是使用消息中的时间戳
    uint64_t systemTimestamp = std::chrono::duration_cast<std::chrono::microseconds>(
        std::chrono::steady_clock::now().time_since_epoch()
    ).count();
    
    // 构建包含完整数据的QVariantMap，使用系统时间戳
    QVariantMap robotData;
    robotData["robotId"] = QString::fromStdString(msg->robot_id);
    robotData["robotType"] = QString::fromStdString(msg->robot_type);
    robotData["x"] = msg->x;
    robotData["y"] = msg->y;
    robotData["status"] = QString::fromStdString(msg->status);
    robotData["battery"] = msg->battery;
    robotData["currentTarget"] = QString::fromStdString(msg->current_target);
    robotData["timestamp"] = QVariant::fromValue<quint64>(systemTimestamp); // 明确指定类型
    robotData["robotIP"] = QString::fromStdString(msg->robot_ip);
    robotData["seerIP"] = QString::fromStdString(msg->seer_ip);
    robotData["cameraUrl"] = QString::fromStdString(msg->camera_url);
    
    // 处理capabilities字段
    QStringList capabilities;
    for (const auto& cap : msg->capabilities) {
        capabilities.append(QString::fromStdString(cap));
    }
    robotData["capabilities"] = capabilities;
/*
    // 构建功能类型列表字符串用于日志输出
    std::string capabilitiesStr = [&]() {
        std::string caps_str = "";
        for (size_t i = 0; i < msg->capabilities.size(); ++i) {
            if (i > 0) caps_str += ", ";
            caps_str += msg->capabilities[i];
        }
        return caps_str.empty() ? "无" : caps_str;
    }();

    LOG_DEBUG("接收到心跳消息 - 机器人ID: {}, 类型: {}, 系统时间戳: {}ms, 状态: {}, 电量: {}%", 
                        msg->robot_id, msg->robot_type, systemTimestamp/1000, msg->status, static_cast<int>(msg->battery));
    LOG_DEBUG("机器人功能类型列表: {}", capabilitiesStr);
*/
    emit heartbeatSignal(robotData);
    lastEmit = now;
}

/**
 * 检查超时函数，用于定期检查心跳超时情况
 * 
 * 此函数通过发出timeoutCheckRequestedSignal信号，启动超时检查流程。
 * 这种设计允许在不同的线程中执行超时检查，提高程序的响应功能类型和效率。
 */
void HeartbeatListener::check_timeout() {
    //LOG_DEBUG("执行超时检查");
    emit timeoutCheckRequestedSignal(timeout_);
}